#include "EstimationStateManager.h"
#include <iostream>
#include <boost/thread.hpp>
#include <stdio.h>
#include "SettingsLoader.h"

#define QUEUE_SIZE 5

EstimationStateManager::EstimationStateManager(VehicleStates* trigger, float* odo, SqLiteDbWrapper* connection, list<Path>* paths, int routeID){

        //Pointer to VSM's State enumeration is inspected for changes every cycle and serves
        //as a rudimentary 'trigger' for communication between threads
        routeTrigger = trigger;
        odometer = odo;
        db = connection;
        pathList = paths;
        int size = Settings::htc.TimeQueueSize;


        nodeTimeQueue = new float[size];

        //Set iterator to last item, so first stop will cycle iterator to first item
        it = pathList->end();
        it--;

        //TODO: Get intervals from config
        avgInterval = 10;
        transmitInterval = 30;

        timeDifference = 0.0;
        distance[1] = distance[0] = linkTravelled = 0;
        time(&avgCntr[0]);
        time(&avgCntr[1]);
};

void EstimationStateManager::operator() (){
    boost::posix_time::milliseconds wait(485);
    bool inService = true;

    while (inService){
    //This sleep determines how quickly the ESM reacts when it reaches a node
    //If set too low, then the thread doesn't relinquish its lock on any shared
    //memory in time and logic errors occur.
    boost::this_thread::sleep(wait);

    if (stateChanged()){
        switch (currentState){//Listen to what the VSM is saying every cycle

        case ATE_STATE_AT_NODE:
        pushNewNode();
        linkTravelled = 0;
        break;

        case ATE_STATE_ON_LINK:
        //ESM idles while vehicle is on link
        break;

        case ATE_STATE_OUT_OF_SERVICE:
        inService = false;
        break;
        }
    }
    updateVelocity();

    } //Loop ends if the VSM reaches its halting state

    //ESM destructor is called multiple times in creation of thread, so dynamic memory is deleted
    //here, only when the ESM has finished
    delete [] nodeTimeQueue;
}

bool EstimationStateManager::stateChanged(){//Observes if the state of the Vehicle has changed
    boost::mutex mutex;
    boost::mutex::scoped_lock lock(mutex);//Ensure access to value at routeTrigger is thread-safe

    if (currentState != *routeTrigger){
        currentState = *routeTrigger;
        return true;
    }

    return false;
}

void EstimationStateManager::pushNewNode(){//Moves the iterator as if the pathList were a circular queue
    cout << "\a";
    it++;

    if (it == pathList->end())
        it = pathList->begin();

}

void EstimationStateManager::updateVelocity(){//Contains logic for making non-threaded calls to core
                                              //Estimation and transmission operations
    boost::mutex mutex;
    boost::mutex::scoped_lock lock(mutex);//Ensure access to value at odo is thread-safe

    distance[0] = *odometer;
    time(&avgCntr[0]);

    //These totals are handled independently of each other, as they are reset under differing conditions
    timeDifference += difftime(avgCntr[0],avgCntr[1]);
    transmitTimeDifference += difftime(avgCntr[0],avgCntr[1]);
    //If Odometer reads negative, correct to zero
    distDifference += ((distance[0] - distance[1]) >= 0) ? (distance[0] - distance[1]) : 0;
    linkTravelled += ((distance[0] - distance[1]) >= 0) ? (distance[0] - distance[1]) : 0;


    if (timeDifference >= avgInterval){//Time condition for Estimation operation

        printf("Average Velocity: %.2f\n", distDifference/timeDifference);
        avgVelocity = estimator.GetEstimate(distDifference/timeDifference);//Push new average into estimator
        printf("Estimation Velocity:  %.2f\n", avgVelocity);

        //Times are only updated when a new average velocity is found
        if (currentState != ATE_STATE_OUT_OF_SERVICE) updateTimeQueue();

        timeDifference = distDifference = 0;
        }

    //Time condition for making a TCP transmission
    if (transmitTimeDifference >= transmitInterval) {/*cout << "TRANSMIT\n"; transmitTimeDifference = 0;*/}
    //HTC::TCP::AddFloatToNextPacket(nodeTimeQueue);

    time(&avgCntr[1]);
    distance[1] = *odometer;
}

void EstimationStateManager::updateTimeQueue(){//Use corrected average to project Estimations to next nodes

    Link link;
    list<Path>::iterator ph;
    ph = it;

    //Initialise time queue
    memset(nodeTimeQueue, 0, sizeof(nodeTimeQueue) * QUEUE_SIZE);

    //Iterate through the next 'n' nodes and compute their ETA using the lengths of the links that connect them
    for(int a = 0; a < QUEUE_SIZE; a++){
        link = db->GetLink(ph->link);

        for (int b = a; b < QUEUE_SIZE; b++)
            nodeTimeQueue[b] += (link.length - ((a == 0)?linkTravelled:0)) / avgVelocity;

        printf("Node %d: %.2f \n", ph->endNode, nodeTimeQueue[a]);

        ph++;
        if (ph == pathList->end())
            ph = pathList->begin();
        }
        cout << "\n";
}

EstimationStateManager::~EstimationStateManager(){
}
